
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_stabilize.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void mode_stabilize_run();
/*----------------------------------variable----------------------------------*/
ModeStabilize mode_stabilize;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static ModeNumber mode_stabilize_mode_number() { return STABILIZE; }
static bool mode_stabilize_requires_GPS() { return false; }
static bool mode_stabilize_has_manual_throttle() { return true; }
static bool mode_stabilize_allows_arming(ArmingMethod method) { return true; };
static bool mode_stabilize_is_autopilot() { return false; }
static bool mode_stabilize_allows_save_trim() { return true; }
static bool mode_stabilize_allows_autotune() { return true; }
static bool mode_stabilize_allows_flip() { return true; }

static const char *mode_stabilize_name() { return "STABILIZE"; }
static const char *mode_stabilize_name4() { return "STAB"; }

Mode* get_mode_stabilize()
{
    return (Mode *)&mode_stabilize;
}

void mode_stabilize_ctor(Mode *mode)
{
    mode_stabilize.mode = *mode;

    mode_stabilize.mode.mode_number = mode_stabilize_mode_number;
    mode_stabilize.mode.requires_GPS = mode_stabilize_requires_GPS;
    mode_stabilize.mode.has_manual_throttle = mode_stabilize_has_manual_throttle;
    mode_stabilize.mode.allows_arming = mode_stabilize_allows_arming;
    mode_stabilize.mode.is_autopilot = mode_stabilize_is_autopilot;
    mode_stabilize.mode.allows_save_trim = mode_stabilize_allows_save_trim;
    mode_stabilize.mode.allows_autotune = mode_stabilize_allows_autotune;
    mode_stabilize.mode.allows_flip = mode_stabilize_allows_flip;
    mode_stabilize.mode.name = mode_stabilize_name;
    mode_stabilize.mode.name4 = mode_stabilize_name4;

    mode_stabilize.mode.run = mode_stabilize_run;
}

// stabilize_run - runs the main stabilize controller
// should be called at 100hz or more
void mode_stabilize_run()
{
    // apply simple mode transform to pilot inputs
    // TODO:

    // convert pilot input to lean angles
    float target_roll, target_pitch;
    fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, fms.g.angle_max, fms.g.angle_max);

    // get pilot's desired yaw rate
    float target_yaw_rate = fms_get_pilot_desired_yaw_rate(RC_norm_input_dz(fms.channel_yaw));

    if (!fms.motors->_armed) {
        // Motors should be Stopped
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_SHUT_DOWN);
    } else if (fms.ap.throttle_zero) {
        // Attempting to Land
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    } else {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    }

    switch (fms.motors->_spool_state) {
    case MOTOR_SHUT_DOWN:
        // Motors Stopped
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
        attctrl_reset_rate_controller_I_terms(fms.attitude_control);
        break;

    case MOTOR_GROUND_IDLE:
        // Landed
        attctrl_reset_yaw_target_and_rate(fms.attitude_control, true);
        attctrl_reset_rate_controller_I_terms_smoothly(fms.attitude_control);
        break;

    case MOTOR_THROTTLE_UNLIMITED:
        // clear landing flag above zero throttle
        if (!fms.motors->limit.throttle_lower) {
            fms_set_land_complete(false);
        }
        break;

    case MOTOR_SPOOLING_UP:
    case MOTOR_SPOOLING_DOWN:
        // do nothing
        break;
    }

    // call attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control,target_roll, target_pitch, target_yaw_rate);

    // output pilot's throttle
    attctrl_set_throttle_out(fms.attitude_control, fms_get_pilot_desired_throttle(), 
                                       true, fms.g.throttle_filt);
}
/*------------------------------------test------------------------------------*/


